	#include "../common/irobot_abstract2.h"
#include "../common/oi.h"

#include <limits.h>
#ifdef SERIAL
#include <math.h>
#endif

#define bound_velocity(x) (x <= 500 ? x : (x >= 0 ? x : -500))
#define side_length 0.8


//create office theme song as song number 0
void create_office_theme_song(void)
{
	ir_send_byte(CmdSong);
	ir_send_byte(0);
	ir_send_byte(14);
	ir_send_byte(67);
	ir_send_byte(32);
	ir_send_byte(79);
	ir_send_byte(64);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(79);
	ir_send_byte(8);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(74);
	ir_send_byte(8);
	ir_send_byte(76);
	ir_send_byte(64);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(69);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(67);
	ir_send_byte(64);
}

//play office theme song
void play_office_theme_song(void)
{
	ir_send_byte(CmdPlay);
	ir_send_byte(0);
}


enum state_t {search,home,halt};

#define A_ERROR 10
#define R_ERROR 300
#define r (258/2 +25)
enum state_t f_search(void){
	static int i = 1;
	static int card_index = 0;
	static int dir = 1;
	static unsigned int card_angle[3] = {UINT_MAX,UINT_MAX,UINT_MAX};
	static unsigned int card_radius[3] = {UINT_MAX,UINT_MAX,UINT_MAX};

	static int cumul_angle = 0;
	cumul_angle += angle();

	if(i < 1 || /*cumul_angle < 0 ||*/ left_bumper() || right_bumper())
		return halt;




	if(angle() > 0){
		if (cumul_angle >= 180){
			++i;
			cumul_angle = 0;
		}
	}
	else {
		if (cumul_angle <= 0){
			--i;
			cumul_angle = 180;
		}
	}

	printf("l: %d\tfl: %d\tfr: %d\tr: %d\n", cliff_left_signal(),
		cliff_front_left_signal(),cliff_front_right_signal(),cliff_right_signal());
	static bool prev_cliff = false;
	if(!prev_cliff && (cliff_left_signal() > 1000 || cliff_right_signal() > 1000 ||
			cliff_front_left_signal() > 1000 || cliff_front_right_signal() > 1000)){


		leds(i%2,true,0,0);
		bool new = true;
		int j;
		for(j = 0; j < card_index; j++){
			new = new && (((card_angle[j] + A_ERROR) < 10+cumul_angle || (card_angle[j] - A_ERROR) > 10+cumul_angle) ||
					card_radius[j] != i-2);
		}

		if(new){
			leds(i%2,true,255,255);
			//play_office_theme_song();
			card_angle[card_index] = cumul_angle;
			card_radius[card_index] = i;
			++card_index;
		}

		if(card_index == 3) return halt;
		/*
		stop();
		if(dir > 0) ir_sleep(1000);
		dir = -1;
		*/
	} else {
		leds(i%2,false,0,0);
	}
	prev_cliff = (cliff_left_signal() > 1000 || cliff_right_signal() > 1000 ||
		cliff_front_left_signal() > 1000 || cliff_front_right_signal() > 1000);


	
	turn_about(dir*150,r*i);
	//turn_about(dir*250,20*0.0174532925*(cumul_angle += angle()));

	return search;
}

enum state_t f_home(void){
	return home;
} 
int main(int argc, char **argv) {
	ir_initialize(0);
	ir_sleep(100);
	
	ir_send_byte(CmdStart);
	ir_send_byte(CmdFull);

/// light the leds--idiot test
	ir_sleep(100);
	leds(false,false,0,255);

	create_office_theme_song();

	enum state_t state = search;
	while(state != halt) {
		ir_sleep(100);
		collect_sensor_data();

		switch (state){
			case search: state= f_search(); break;
			case home: state= f_home(); break;
		}
	}

	stop();
	leds(0,0,255,255);
	//play_office_theme_song();

	ir_finish();
}
